#include<ros/ros.h>
#include<turtlesim/Pose.h>
#include"tf2_ros/transform_broadcaster.h"
#include<geometry_msgs/TransformStamped.h>
#include<tf2/LinearMath/Quaternion.h>



void doPose(const turtlesim::Pose::ConstPtr &pose){
    //获取位姿 转换成坐标系相对
    //a.创建发布对象
    static tf2_ros::TransformBroadcaster pub;//static 每次调用时不用生产新的对象

    //b.组织发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";

    //坐标偏移量
    ts.transform.translation.x  = pose->x;
    ts.transform.translation.y  = pose->y;
    ts.transform.translation.z  = 0;
    //坐标系四元数
    /*
        位姿信息中没有四元数，但是有个偏航角度，又已知龟龟是2D 没有翻滚 和 俯仰
    */
    //pose->theta 乌龟的俯仰
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);

    ts.transform.rotation.x = qtn.getX();
    
    ts.transform.rotation.y = qtn.getY();

    ts.transform.rotation.z = qtn.getZ();

    ts.transform.rotation.w = qtn.getW();



    //c.发布
    pub.sendTransform(ts);

}


/*



    1、包含头文件
    2、设置编码
    3、创建订阅对象，订阅 \turtle1\pose
    4、回调函数
    5、spin()

*/

int main(int argc, char *argv[])
{   
    setlocale(LC_ALL,"");
    ros::init(argc, argv,"dynamic_pub");

    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);

    ros::spin();

    return 0;
}
